#include "encoder.h"
#include "gpio.h"
#include "timer.h"

// 定义两个编码器的值
volatile int16_t Encoder_Left = 0;
volatile int16_t Encoder_Right = 0;

// 两个编码器同时初始化
void Encoder_Init(void)
{
    // ====================================左(A)电机编码器====================================
    pcnt_config_t pcnt_config = {
        .pulse_gpio_num = GPIO_PIN_35,          // 计数端口----------A相
        .ctrl_gpio_num = GPIO_PIN_39,           // 方向检测端口-------B相
        .lctrl_mode = PCNT_MODE_REVERSE,
        .hctrl_mode = PCNT_MODE_KEEP,
        .pos_mode = PCNT_COUNT_INC,
        .neg_mode = PCNT_COUNT_DEC,
        .counter_h_lim = 32767,
        .counter_l_lim = -32767,
        .unit = PCNT_UNIT_0,
        .channel = PCNT_CHANNEL_0
    };

    // 初始化
    pcnt_unit_config(&pcnt_config);
    pcnt_counter_pause(PCNT_UNIT_0);
    pcnt_counter_clear(PCNT_UNIT_0);
    pcnt_counter_resume(PCNT_UNIT_0);
    // ====================================左(A)电机编码器====================================

    // ====================================右(B)电机编码器====================================
    pcnt_config = {
        .pulse_gpio_num = GPIO_PIN_36,         // 计数端口----------A相
        .ctrl_gpio_num = GPIO_PIN_34,          // 方向检测端口-------B相
        .lctrl_mode = PCNT_MODE_REVERSE,
        .hctrl_mode = PCNT_MODE_KEEP,
        .pos_mode = PCNT_COUNT_INC,
        .neg_mode = PCNT_COUNT_DEC,
        .counter_h_lim = 32767,
        .counter_l_lim = -32767,
        .unit = PCNT_UNIT_1,
        .channel = PCNT_CHANNEL_0
    };

    // 初始化
    pcnt_unit_config(&pcnt_config);
    pcnt_counter_pause(PCNT_UNIT_1);
    pcnt_counter_clear(PCNT_UNIT_1);
    pcnt_counter_resume(PCNT_UNIT_1);
    // ====================================右(B)电机编码器====================================
    
}

// 获取编码器计数值
int16_t Encoder_GetCount(Encoder_M Encoder_Num)
{
    int16_t count;

    if(Encoder_Num == Encoder_M1)
    {
         pcnt_get_counter_value(PCNT_UNIT_0, &count);  // 读取计数值
         pcnt_counter_clear(PCNT_UNIT_0);              // 清零计数器
    }

    if(Encoder_Num == Encoder_M2)
    {
         pcnt_get_counter_value(PCNT_UNIT_1, &count);  // 读取计数值
         pcnt_counter_clear(PCNT_UNIT_1);              // 清零计数器
    }
   
    return count;
}

void TIM_Interrupt_1(void)
{
    Encoder_Left = Encoder_GetCount(Encoder_M1);
    Encoder_Right = Encoder_GetCount(Encoder_M2);
}